perm filename ARMSOL[SYS,HE] blob
sn#171577 filedate 1975-07-31 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00015 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 TITLE ARMSOL
C00003 00003 THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
C00004 00004 BEGIN CROSS
C00005 00005 BEGIN SECOND
C00006 00006 BEGIN ANGLE
C00008 00007 BEGIN SOLVE
C00012 00008 WE NOW NEED SOME SINES AND COSINES
C00014 00009 NOW JOINT 4
C00017 00010 DEGEN: SETZM J+4 JOINT 5 IS ZERO
C00018 00011 FLAG: 0
C00019 00012 IFE WAVE,<
C00022 00013 MUL: MOVEI J,2
C00023 00014 TH: BLOCK 6
C00024 00015 BEGIN TRANSMULT
C00025 ENDMK
C⊗;
TITLE ARMSOL
↓WAVE←←0 ;just assemble the solution for wave
↓BLUE←←0 ;assemble for blue arm
IFN BLUE,<
↓LH←←1 ;left hand arm (blue)
↓RH←←0 ;right hand arm (yellow)
>
IFE BLUE,<
↓LH←←0 ;left hand arm (blue)
↓RH←←1 ;right hand arm (yellow)
>
;THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
IFN WAVE,<
INTERNAL ARM.SOLVE
>
IFE WAVE,<
INTERNAL ANGLE,SOLVE,UPDATE,TIMES
>
EXTERNAL ACOS,SQRT,ATAN2,ASIN,ACOS,SIND,COSD
↓AC←1
↓P←17
IFE BLUE,<
S2V←←6.05
S6V←←9.38
>
IFN BLUE,<
S2V←←-6.05
S6V←←9.425
>
S22V←←36.6025
↓RAD: 206712273406
IFN BLUE,<
↓SHOLDER:20.035
51.305
20.24
>
IFE BLUE,<
↓SHOLDER:20.035
10.305
16.24
>
BEGIN CROSS
;CROSS(R,P,A) RETURNS THE CROSS PRODUCT IN R
;TIME APPROX 180 MICRO SECONDS
TAC1←←2
PV1←←3
PV2←←4
PV3←←5
AV1←←6
AV2←←7
AV3←←10
O←←11
↑CROSS:
HRL AC,-2(P) ;MOVE PV INTO AC'S
HRRI AC,PV1
BLT AC,PV3
HRL AC,-1(P) ;MOVE AV INTO AC'S
HRRI AC,AV1
BLT AC,AV3
MOVE O,-3(P)
MOVE AC,PV2
FMPR AC,AV3
MOVE TAC1,PV3
FMPR TAC1,AV2
FSBR AC,TAC1
MOVEM AC,(O)
FMPR PV3,AV1
FMPR AV3,PV1
FSBR PV3,AV3
MOVEM PV3,1(O)
FMPR PV1,AV2
FMPR AV1,PV2
FSBR PV1,AV1
MOVEM PV1,2(O)
SUB P,[4(4)]
JRST @4(P)
BEND
BEGIN SECOND
;SECOND(A) RETURNS THE MAGNITUDE SQUARED OF THE VECTOR
;TIME APPROX 90 MICRO SEC
A←←4
↑SECOND:
HRL A,-1(P)
HRRI A,AC
BLT A,AC+2
FMPR AC,AC
FMPR AC+1,AC+1
FMPR AC+2,AC+2
FADR AC,AC+1
FADR AC,AC+2
SUB P,[2(2)]
JRST @2(P)
BEND
BEGIN ANGLE
;ANGLE(P,A,O) RETURNS THE ANGLE BETWEEN P AND A IN DEGREES ABOUT O
;A AND P MUST BE UNIT VECTORS. I.E. DIRECTION COSINES
;COMPUTES THE ANGLE=ACOS(PV1*AV1 + PV2*AV2 + PV3*AV3)
;AND THEN THE DETERMINANT
; | O1 O2 O3|
; |PV1 PV2 PV3|
; |AV1 AV2 AV3|
;AND IF POSITIVE MULTIPLIES THE ANGLE BY -1;
;TIME APPROX 550 MICRO SECONDS
TAC1←←2
PV1←←3
PV2←←4
PV3←←5
AV1←←6
AV2←←7
AV3←←10
O←←11
I←←13 ;12 IS A NO NO
↑ANGLE:
HRL AC,-3(P) ;MOVE PV INTO AC'S
HRRI AC,PV1
BLT AC,PV3
HRL AC,-2(P) ;MOVE AV INTO AC'S
HRRI AC,AV1
BLT AC,AV3
MOVEI I,2 ;COMPUTE DOT PRODUCT
MOVE AC,PV1
FMPR AC,AV1
L1: MOVE TAC1,PV1(I)
FMPR TAC1,AV1(I)
FADR AC,TAC1
SOJG I,L1
MOVEM AC,DOT
MOVE O,-1(P) ;COMPUTE DETERMINANT
MOVE AC,PV2
FMPR AC,AV3
MOVE TAC1,PV3
FMPR TAC1,AV2
FSBR AC,TAC1
FMPR AC,(O)
FMPR PV3,AV1
FMPR AV3,PV1
FSBR PV3,AV3
FMPR PV3,1(O)
FADR AC,PV3
FMPR PV1,AV2
FMPR AV1,PV2
FSBR PV1,AV1
FMPR PV1,2(O)
FADR AC,PV1
MOVEM AC,DET
PUSH P,DOT
PUSHJ P,ACOS
FMPR AC,RAD
SKIPL DET ;NEGATE ANGLE IF DET NEGATIVE
MOVN AC,AC
SUB P,[4(4)]
JRST @4(P)
DOT: 0
DET: 0
BEND
BEGIN SOLVE
;SOLVE(T,J) GIVEN THE TRANSFORM T, RETURNS THE SOLUTION IN "J"
;RETURNS FALSE IF THE SOLUTION IS CORRECT
;TIME APPROX 4 M. SEC
TAC←←2
I←←3
IFE WAVE,<
↑SOLVE:
>
IFN WAVE,<
↑ARM.SOLVE:
>
SETZM FLAG ;SET RETURN FLAG TRUE
HRL AC,-1(P) ;INITIALIZE J
HRRI AC,J
BLT AC,J+5
;COMPUTE W=P-SHOLDER-A*S6
HRRZ AC,-2(P) ;MOVE P INTO MEMORY
MOVE TAC,3(AC)
MOVEM TAC,PV1
MOVE TAC,7(AC)
MOVEM TAC,PV1+1
MOVE TAC,13(AC)
MOVEM TAC,PV1+2
MOVE TAC,2(AC) ;MOVE A INTO MEMORY
MOVEM TAC,AV1
MOVE TAC,6(AC)
MOVEM TAC,AV1+1
MOVE TAC,12(AC)
MOVEM TAC,AV1+2
MOVE TAC,1(AC) ;MOVE O INTO MEMORY
MOVEM TAC,OV1
MOVE TAC,5(AC)
MOVEM TAC,OV1+1
MOVE TAC,11(AC)
MOVEM TAC,OV1+2
;J2=DOT(W,W)-S2↑2
MOVN TAC,S22 ;INITIALIZE TAC TO -S22
MOVEI I,2 ;COMPUTE THE DOT PRODUCT
L1: MOVE AC,AV1(I)
FMPR AC,S6
FADR AC,SHOLDER(I)
FSBR AC,PV1(I)
MOVEM AC,W(I) ;-W(I)
FMPR AC,AC
MOVEM AC,W2(I) ;W(I)↑2
FADR TAC,AC
SOJGE I,L1
JUMPLE TAC,[SETZ AC,
JRST NSR]
PUSH P,TAC
PUSHJ P,SQRT
NSR: CAMLE AC,STOP+3 ;CHECK THE STOPS
JRST[ MOVE AC,STOP+3
JRST M3]
CAMGE AC,STOP+2
JRST[ MOVE AC,STOP+2
M3: SETOM FLAG
FADR AC,J+2
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK3]
OK3: MOVEM AC,J+2 ;JOINT THREE
;NOW COMPUTE THE BOOM OFFSET
MOVE AC,W2+1
FADR AC,W2
PUSH P,AC
PUSHJ P,SQRT
MOVM TAC,S2
FDVR TAC,AC
CAML TAC,[1.0]
MOVSI TAC,(1.0)
PUSH P,TAC
PUSHJ P,ASIN
MOVEM AC,JTEMP
;NOW COMPUTE TH FOR JOINT 1
PUSH P,W+1
PUSH P,W
PUSHJ P,ATAN2
IFN RH,<
FADR AC,JTEMP ;ADD J
>
IFN LH,<
FSBR AC,JTEMP
>
FMPR AC,RAD
MOVE TAC,AC
IFN RH,<
FSBR TAC,[90.0]
JUMPL TAC,ISOK
FSBR AC,[360.0]
>
IFN LH,<
FADR TAC,[90.0]
JUMPG TAC,ISOK
FADR AC,[360.0]
>
ISOK: CAMLE AC,STOP+1 ;AND CHECK THE STOPS FOR JOINT 1
JRST[ MOVE AC,STOP+1
JRST M1]
CAMGE AC,STOP
JRST[ MOVE AC,STOP
M1: SETOM FLAG
FADR AC,J
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK1]
OK1: MOVEM AC,J ;JOINT ONE
;JOINT 2-ACOS(W3/J3)
MOVN AC,W+2
FDVR AC,J+2
MOVEM AC,CO2
PUSH P,AC
PUSHJ P,ACOS
FMPR AC,RAD
MOVNM AC,J+1 ;JOINT TWO
;WE NOW NEED SOME SINES AND COSINES
PUSH P,J
PUSHJ P,SIND
MOVEM AC,SI1
PUSH P,J
PUSHJ P,COSD
MOVEM AC,CO1
PUSH P,J+1
PUSHJ P,SIND
MOVEM AC,SI2
;WE NOW COMPUTE THE UNIT VECTORS AT THE END OF LINK 3
MOVE AC,CO1
FMPR AC,CO2
MOVEM AC,Y
MOVE AC,SI1
FMPR AC,CO2
MOVEM AC,Y+1
MOVE AC,SI2
MOVNM AC,Y+2
FMPR AC,CO1
MOVEM AC,Z
MOVE AC,SI1
FMPR AC,SI2
MOVEM AC,Z+1
MOVE AC,CO2
MOVEM AC,Z+2
;CROSS(VT,Z,A);
PUSH P,[VT]
PUSH P,[Z]
PUSH P,[AV1]
PUSHJ P,CROSS
;CHECK FOR DEGENERATE CASE
PUSH P,[VT]
PUSHJ P,SECOND
CAMG AC,[0.000001]
JRST DEGEN ;JOINTS 4 AND 6 ARE DEGENERATE
PUSH P,AC
PUSHJ P,SQRT ;GET MAGNITUDE OF VT
MOVEI I,2
FVT: MOVE TAC,VT(I)
FDVR TAC,AC
MOVEM TAC,VT(I)
SOJGE I,FVT
VTF: ;VT NOW UNIT MAGNITUDE
;NOW JOINT 4
PUSH P,[VT]
PUSH P,[Y]
PUSH P,[Z]
PUSHJ P,ANGLE
MOVE TAC,AC
FSBR TAC,J+3
CAMLE TAC,[180.0]
JRST[ FSBR AC,[360.0]
JRST TN]
CAMGE TAC,[-180.0]
JRST[ FADR AC,[360.0]
JRST TN]
TN:
MOVE TAC,AC
FSBR TAC,J+3
CAMLE TAC,[90.0]
JRST[ FSBR AC,[180.0]
JRST RVTO]
CAMGE TAC,[-90.0]
JRST[ FADR AC,[180.0]
RVTO: MOVNS VT
MOVNS VT+1
MOVNS VT+2
JRST SOK4]
SOK4:
;NOW CHECK THE STOPS
CAMLE AC,STOP+5
JRST[ FSBR AC,[180.0]
JRST RVT]
CAMGE AC,STOP+4
JRST[ FADR AC,[180.0]
RVT: MOVNS VT
MOVNS VT+1
MOVNS VT+2
JRST OK4]
OK4: MOVEM AC,J+3 ;JOINT FOUR
;NOW JOINT 5
PUSH P,[AV1]
PUSH P,[Z]
PUSH P,[VT]
PUSHJ P,ANGLE
CiON]e~R>A[84(&U∩NRlLj>Z∃∧
2N$zA-\hP$&*∃~Qα5-h4(&≤
6≡∃∧
2N$zA-XhP&*J≥"l&6⎇2∃αε~bNR>αYX4(LiUh&≤*R>5∧22ε≤hP$&~ "Iαε~b)-PhP$&~≤→αε
biD%nD
2→α<
eα
-"↑⊗⊗rαNR>ααε:⊃∧bεNQ∧R>&:"αε:≡d(4($LRJNQ∧z-VthR>-UPJ6>Z,iαε
dQ-P4SZ*>&u!↓YαM→α:>:αN&6∧b∀4(MαVN!¬↓2n>3
t4(MαVN!¬↓2nZ%h4(&¬*N!ααbnεY
h4(&¬*N")¬↓2ε:<b∀4*$z1h&lzZ∃α$
2ε_h(&~≤∩IαR →2)-(h(&∞ j1αR →2mEC↓9BthP&*J≥"l&~≤∩Iαε~bmMYαqBt4PH&*J≥!αNQ5h4(&≤
6≡∃¬"ε
2ZiEaAsαt4(LRJNRXJ~ε∩∩αε
2[→YA9¬h4($LRJNQ¬~QZthRNQYPJ∞ε6d)αε
e~R>A[ D4(LRJNRXJ~N
∩αε
2[→YA9¬h4($LRJNQ∧z-ZthP&∞εl:∃αε~bNR>αYE@4PJ~ε∩∩αε
2[→YA9¬h4*>[1h&6⎇2⊗5α →2)-(h(&"∃∩iαε~a5E"αH4(&E∩Jj%¬"ε
1D
$4PJ"J2Jαε
2Ph(&
e!αε
c)"Rε~H4(4PJ6>Z*αε
24bε≤4PJNV ¬↓2mMC→&t4PJ*JN"α↓M"αH4(4P2∩⊗<*9h&≤*Rj5∧Q-P$KZ*>&u!↓UαM→αj⊗∀x4(&¬*N!ααbn>Y
h4(&¬*N!ααbnfthP&BV≤AαA2\
YFthP&BV≤B)αAd
:≡2(h(&~≤∩Iαε~b)-LHIn2⊗
2∃α*|J:Q↓"αεMαM_4(&U∩NQα$z0$%\
:⊃α¬∩>∞⊗,!αεM¬*NVε`h(2~2 9h%@hR*R⊗m↓h%@hRBYEPJ
2>≤Y↓L4T
YEhL∩2>∞Z↓L4*⎇1Eh&∀b>∞-β_4*]PI@4*≤IEh%h*∞= P%@4U9Ih%h*N%∪P%@4T~=IhK4*Z#P&
2|~-↓LhReh&∀b>∞-β_4*iPJ
2>≤Y↓L4TQh&
dz∞-↓0h*MIPJMJXhRMIIPJMIJ0h*MYPJMZXhRRB%PIIAM3⊃IA];→IT4U~R>APh*&~rα
2V*ap4(JieA9h(%EK↓9@4PIY9HhP%MQs⊂4(%k→IU9h(%I≠)9@4PI5EA*q@4(K AU9h(%5≠)A9@hP%IIαq@4)ph*&~*α
2V*ap4(JiEaUs4(%3↓9@4PIY9ThP%I]s(4(%k ]U9h(%E#↓9@4PI5EA
q@4(K AE9h(%5∪9A9@hP%I]αq@4)ph(&
,r⊂4(hP2&~*α↑εZ*ap4(L∩⊗≡&rαVB∩
"∀4)]*B∩ε$)"Q2RH4)n<JZ⊗9∧R>&:"αε:≡d*Mα)∧~>6B-"⊗Mα$B∃αR∀
:N~⎇∩6εRLz9αPhQnR&l)αεB¬∩>a↓2α59α≤* 4(hP&¬F⎇yH4(L J}⎇_h(&¬≥z⎇P4PJIF}{(4(&∪∩}⎇XhP&IN⎇y\4(LJ}⎇Eh(&*⎇yED4PJ.}⎇ ⊂4(4UrVB∩
"∃h4PJ"J1∧
15
BA$$KZR 4PJ"JJJαε
2$@4(&∀bQαε~bR!-(h(&N-"j5α H$%n≤*QαQ¬"=αVtJQα6
"J&`hP&6>4)αε
eZb↑⊃¬!2Q-
h4(&∀bQαε~bQ-E_h(&6⎇2N%α →1!Es↓$4(Lj>Z⊗jαε
2 h(&6⎇2⊗5α →2Q-(h(&6⎇2⊗5α →2Q- ⊂4(&≤*Rj5∧b&:,hR6εRm*1h$HH%n6
"6V0M!"-&⎇!"-5
I*¬"ZH4(&lzZ∃α →2nb<!αQ2$rt4(L∩2Qα →2R9[ L$%\j>Z∃¬!α&:$yαR8hP&6>4)α-2dJ:,4PJ∞ε&rα-1HhP&*J≥"l&6⎇2∃αε~bN%LhP$&6⎇2⊗5α →2N$hP$&6⎇2∃αε~b∞=LhP$&6⎇2⊗5α →2∞<hP$&6⎇2∃αε~bR!-⊂h($&lzZ⊗5∧
2M[⊂4($LRJNQ∧J∩NVhh(&B-~!αAe"!"-Hh(&B-~")ααbN&: h(&6⎇2⊗5α →2N$hP&6>4)α-2dJ:,4PJBVNBαA2RBB-$4PJBVNDQαA2≤zN⊂4PJ6>Z,iαε
d~<4*L"NUhLj>Z∃∧Y22&tX4)n4JJNQ¬∩>\4PJ6>Z*α¬E2≤x4(&lzZ∃α ⊃2N$hP&N⊗%Qα¬M`h(&6⎇2⊗%αJa@4(MαVN"RαA26,`4)n≤*∞>:"αJ>\hP&6>4qα¬Ed~¬"-Hh(&~mαIα¬
bN$4PJ6>Z*α¬I2≤ "-$hP&~6¬⊃α¬Id~<4(Lj>Z∃∧ M2N
B-$4PJ6>Z,Iα%1λh(&B-~")ααb6V0hQnR"M∩⊃αJ⎇84(&lzZ∃α 2N¬DY$4(L26BI∧ E2NHh(&6⎇29α¬∩bN¬"ZH4(&4jBIα ⊃2∞<hP&6>4)α¬Md~¬"-Hh(&6⎇2⊗%αJaH4(MαVN"RαA26,`4)n4zVJRBαJ>\hP&N⊗%Qα¬E`h(&N-"iα¬∩`4(&lzZ∃α →2M"ZH4(&lzZ⊗%∧I1L4PJBVNDQαA2m*04(Lj>Z∃∧
2RrYL$%\z2⊃α@h(&~ "J5α →2Q-_H%n:-9α`4PJ6>Z*αε
2$suqL+7 ;OLD Y
FADRM AC,T+7 ;NEW Y
MOVE AC,TN+13 ;OLD Z
FADRM AC,T+13 ;NEW Z
AOS K,LINK
CAIGE K,6
JRST MATMUL
HRRZ K,-2(P)
MOVEI AC,13(K)
HRLI K,T
BLT K,(AC)
HRRZ K,-2(P)
MOVE AC,SHOLDER
FADRM AC,3(K) ;X
MOVE AC,SHOLDER+1
FADRM AC,7(K) ;Y
MOVE AC,SHOLDER+2
FADRM AC,13(K) ;Z
SUB P,[3(3)]
JRST @3(P)
MUL: MOVEI J,2
L2: MOVE AC,PW(J)
BLT AC,R1+2
FMPR R1,A1
FMPR R2,A2
FMPR R3,A3
FADR R1,R2
FADR R1,R3
MOVEM R1,T(I)
ADDI I,4
SOJGE J,L2
POPJ P,
TH: BLOCK 6
PW: XWD TN+10,R1
XWD TN+4,R1
XWD TN,R1
LINK: 0
SI: 0
CO: 0
T: BLOCK 14
TN: BLOCK 14
SI3: -1.0
CO3: 0.0
S: 0.0
S2V
0.0
0.0
0.0
S6V
SA: -1.0
1.0
0.0
-1.0
1.0
0.0
CA: 0.0
0.0
1.0
0.0
0.0
1.0
BEND
BEGIN TRANSMULT
S←1
M←2
A1←3
A2←4
A3←5
A4←6
I←7
J←10
R←11
T←14
U←13
↑TIMES:HRL T,-1(P)
HRRI T,MLT
BLT T,MLT+13
MOVEI T,MLT
HRRZ U,-2(P)
HRRZ R,-3(P)
MOVEI I,3
L1: MOVEI J,3
MOVE A1,(U)
MOVE A2,1(U)
MOVE A3,2(U)
L2: MOVE S,A1
FMPR S,(T)
MOVE M,A2
FMPR M,4(T)
FADR S,M
MOVE M,A3
FMPR M,10(T)
FADR S,M
MOVEM S,(R)
AOJ R,
AOJ T,
SOJG J,L2
FMPR A1,(T)
FMPR A2,4(T)
FADR A1,A2
FMPR A3,10(T)
FADR A1,A3
FADR A1,3(U)
MOVEM A1,(R)
AOJ R,
SUBI T,3
ADDI U,4
SOJG I,L1
SUB P,[XWD 4,4]
JRST @4(P)
MLT: BLOCK 14
BEND
>
END